#include <pic16/pic18f2550.h>
#include <pic16/adc.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <usart.h>
#include "stepper_test.h"

#define DEBUG

/**** Global Variables ****/
volatile unsigned char enable_a, enable_b;
volatile unsigned char out_a1, out_a2;
volatile unsigned char out_b1, out_b2;

volatile unsigned char duty_cycle_pattern;
volatile unsigned char duty_cycle_index;
volatile unsigned long timer_2;

volatile unsigned char duty_cycle;

void main(void) {
	unsigned char i;
	OSCCONbits.SCS = 0x0;		// System Clock Select bits = External oscillator
	OSCCONbits.IRCF = 0x7;		// Internal Oscillator Frequency Select bits 8 MHz (INTOSC drives clock directly)

	UCONbits.USBEN = 0;
	UCFGbits.UTRDIS = 1;
	
	TRIS_OUT_A1 = 0;			// as output
	TRIS_OUT_A2	= 0;			// as output
	TRIS_ENABLE_A = 0;			// as output
	TRIS_OUT_B1	= 0;			// as output
	TRIS_OUT_B2	= 0;			// as output
	TRIS_ENABLE_B = 0;			// as output

	OUT_A1 = 0;
	OUT_A2 = 0;
	OUT_B1 = 0;
	OUT_B2 = 0;
	ENABLE_A = 0;
	ENABLE_B = 0;

	out_a1 = 0;
	out_a2 = 0;
	out_b1 = 0;
	out_b2 = 0;
	
	duty_cycle_pattern = 0;
	timer_2 = 0;

	// set up interrupt and timers
	RCONbits.IPEN = 1;

	// timer 0
	T0CONbits.T08BIT = 0;	// use timer0 16-bit counter
	T0CONbits.T0CS = 0;		// internal clock source
	T0CONbits.PSA = 1;		// disable timer0 prescaler
	INTCON2bits.TMR0IP = 1;	// high priority
	INTCONbits.TMR0IF = 1;	/* Force Instant entry to Timer 0 Interrupt */
	T0CONbits.TMR0ON = 1;	// enable timer0
	INTCONbits.T0IE = 1;	/* Ensure that TMR0 Interrupt is enabled    */

	T1CONbits.T1OSCEN = 0;	// dont put t1 on pin

	// timer 2
	T2CONbits.T2CKPS0 = 1;
	T2CONbits.T2CKPS1 = 0;
	T2CONbits.TOUTPS0 = 1;
	T2CONbits.TOUTPS1 = 0;
	T2CONbits.TOUTPS2 = 0;
	T2CONbits.TOUTPS3 = 1;
	IPR1bits.TMR2IP = 0;		// low priority
	PIR1bits.TMR2IF = 1;
	T2CONbits.TMR2ON = 1;
	PIE1bits.TMR2IE = 1;

	INTCONbits.PEIE = 1;
	INTCONbits.GIE = 1;

	sleep_ms(100);

	i = MIN_SPEED;
	while (1) {
		while (i < MAX_SPEED) {
			stepper_cw(1, i, 20 + (80 * i / 255));
			i++;
		}
		stepper_cw(48, i, 20 + (80 * i / 255));
		while (i > MIN_SPEED) {
			stepper_cw(1, i, 20 + (80 * i / 255));
			i--;
		}
	}
}

void stepper_cw(unsigned int steps, unsigned char speed, unsigned char duty) {
	unsigned int i;
	duty_cycle = duty;
	INTCONbits.T0IE = 0;
	for (i = 0; i < steps; i++) {
		ENABLE_A = 1;
		ENABLE_B = 0;
		INTCONbits.T0IE = 0;
		out_a1 = 1;
		out_a2 = 0;
		INTCONbits.T0IE = 1;
		INTCONbits.TMR0IF = 1;
		sleep_ms(0xff - speed + ON_TIME);
		ENABLE_A = 0;
		ENABLE_B = 0;
		sleep_ms(OFF_TIME);

		ENABLE_A = 0;
		ENABLE_B = 1;
		INTCONbits.T0IE = 0;
		out_b1 = 1;
		out_b2 = 0;
		INTCONbits.T0IE = 1;
		INTCONbits.TMR0IF = 1;
		sleep_ms(0xff - speed + ON_TIME);
		ENABLE_A = 0;
		ENABLE_B = 0;
		sleep_ms(OFF_TIME);

		ENABLE_A = 1;
		ENABLE_B = 0;
		INTCONbits.T0IE = 0;
		out_a1 = 0;
		out_a2 = 1;
		INTCONbits.T0IE = 1;
		INTCONbits.TMR0IF = 1;
		sleep_ms(0xff - speed + ON_TIME);
		ENABLE_A = 0;
		ENABLE_B = 0;
		sleep_ms(OFF_TIME);

		ENABLE_A = 0;
		ENABLE_B = 1;
		INTCONbits.T0IE = 0;
		out_b1 = 0;
		out_b2 = 1;
		INTCONbits.T0IE = 1;
		INTCONbits.TMR0IF = 1;
		sleep_ms(0xff - speed + ON_TIME);
		ENABLE_A = 0;
		ENABLE_B = 0;
		sleep_ms(OFF_TIME);
	}
	INTCONbits.T0IE = 1;
}

void stepper_ccw(unsigned int steps, unsigned char speed, unsigned char duty) {
	unsigned int i;
	duty_cycle = duty;
	INTCONbits.T0IE = 0;
	for (i = 0; i < steps; i++) {
		ENABLE_A = 1;
		ENABLE_B = 0;
		INTCONbits.T0IE = 0;
		out_a1 = 1;
		out_a2 = 0;
		INTCONbits.T0IE = 1;
		INTCONbits.TMR0IF = 1;
		sleep_ms(0xff - speed + ON_TIME);
		ENABLE_A = 0;
		ENABLE_B = 0;
		sleep_ms(OFF_TIME);

		ENABLE_A = 0;
		ENABLE_B = 1;
		INTCONbits.T0IE = 0;
		out_b1 = 0;
		out_b2 = 1;
		INTCONbits.T0IE = 1;
		INTCONbits.TMR0IF = 1;
		sleep_ms(0xff - speed + ON_TIME);
		ENABLE_A = 0;
		ENABLE_B = 0;
		sleep_ms(OFF_TIME);

		ENABLE_A = 1;
		ENABLE_B = 0;
		INTCONbits.T0IE = 0;
		out_a1 = 0;
		out_a2 = 1;
		INTCONbits.T0IE = 1;
		INTCONbits.TMR0IF = 1;
		sleep_ms(0xff - speed + ON_TIME);
		ENABLE_A = 0;
		ENABLE_B = 0;
		sleep_ms(OFF_TIME);

		ENABLE_A = 0;
		ENABLE_B = 1;
		INTCONbits.T0IE = 0;
		out_b1 = 1;
		out_b2 = 0;
		INTCONbits.T0IE = 1;
		INTCONbits.TMR0IF = 1;
		sleep_ms(0xff - speed + ON_TIME);
		ENABLE_A = 0;
		ENABLE_B = 0;
		sleep_ms(OFF_TIME);
	}
	INTCONbits.T0IE = 1;
}

void sleep_ms(unsigned long ms) {
	unsigned long start_timer_2;

	start_timer_2 = timer_2;
	

// while the absolute value of the time diff < ms
	while ( (((signed long)(timer_2 - start_timer_2) < 0) ? (-1 * (timer_2 - start_timer_2)) : (timer_2 - start_timer_2)) < ms) {
		// do nothing
	}
}

static void pwm_control(void) __interrupt 1 {
	if (INTCONbits.TMR0IF) {	// 416,6 us delay at 20 MHz
		TMR0H = (unsigned char)(TIMER0_RELOAD >> 8);
		TMR0L = (unsigned char)TIMER0_RELOAD;	/* Reload the Timer ASAP */
		INTCONbits.TMR0IF = 0;	/* Clear the Timer Flag  */
		do_pwm();
	 }
}

static void timer_control(void) __interrupt 2 {
	if (PIR1bits.TMR2IF) {
		PR2 = TIMER2_RELOAD;		// 1 ms delay at 8 MHz
		PIR1bits.TMR2IF = 0;
		timer_2++;
	}
}

void do_pwm() {
	unsigned char bit;
	unsigned char pattern;
	unsigned char i;
	unsigned char num_on_bits;

	pattern = 0;
	num_on_bits = (8 * duty_cycle / 100);
	for (i = 0; i < num_on_bits; i++) {
		pattern |= 1 << i;
	}

	// called with new duty cycle
	if (duty_cycle_pattern != pattern) {
		duty_cycle_pattern = pattern;
		duty_cycle_index = 0;
	}

	bit = (duty_cycle_pattern & (1 << duty_cycle_index)) >> duty_cycle_index;

	if (out_a1 > 0) {
		OUT_A1 = bit;
	}
	else {
		OUT_A1 = 0;
	}

	if (out_a2 > 0) {
		OUT_A2 = bit;
	}
	else {
		OUT_A2 = 0;
	}

	if (out_b1 > 0) {
		OUT_B1 = bit;
	}
	else {
		OUT_B1 = 0;
	}

	if (out_b2 > 0) {
		OUT_B2 = bit;
	}
	else {
		OUT_B2 = 0;
	}
	duty_cycle_index++;
	duty_cycle_index &= 0x7;	// 00000111
}

